/****************************************************************
 * Fulguro
 * Copyright (C) 2004 Christophe Clienti
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 ***************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <linux/types.h>
#include <linux/videodev.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/file.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "flgrRtCapture.h"

#include <flgrCoreDispatch.h>

/*!
 * \defgroup group_fulguro_realtimetools Realtime Tools
 * \brief Some tools to acquire and display video
 * \defgroup group_fulguro_realtimetools_vcap Video Capture
 * \brief Video capture device management
 * \ingroup group_fulguro_realtimetools
 * @{
 */


////////////////////////////////////////////////////////////////////////////
/*! Return palette FLAG corresponding to a palette string
 *  @param *palette : string for palette.
 *  @return palette flag as defined in videodev.h
 */
////////////////////////////////////////////////////////////////////////////
unsigned short flgr2d_rtcapture_palette_from_string(char *palette) {
  FLGR_DECLARE_FUNCTION;

  if(strcmp(palette,"GREY")==0) {
    return VIDEO_PALETTE_GREY;

  }else if(strcmp(palette,"HI240")==0) {
    return VIDEO_PALETTE_HI240;

  }else if(strcmp(palette,"RGB565")==0) {
    return VIDEO_PALETTE_RGB565;

  }else if(strcmp(palette,"RGB555")==0) {
    return VIDEO_PALETTE_RGB555;

  }else if(strcmp(palette,"RGB24")==0) {
    return VIDEO_PALETTE_RGB24;

  }else if(strcmp(palette,"RGB32")==0) {
    return VIDEO_PALETTE_RGB32;

  }else if(strcmp(palette,"YUV422")==0) {
    return VIDEO_PALETTE_YUV422;

  }else if(strcmp(palette,"YUYV")==0) {
    return VIDEO_PALETTE_YUYV;

  }else if(strcmp(palette,"UYVY")==0) {
    return VIDEO_PALETTE_UYVY;

  }else if(strcmp(palette,"YUV420")==0) {
    return VIDEO_PALETTE_YUV420;

  }else if(strcmp(palette,"YUV411")==0) {
    return VIDEO_PALETTE_YUV411;

  }else if(strcmp(palette,"RAW")==0) {
    return VIDEO_PALETTE_RAW;

  }else if(strcmp(palette,"YUV422P")==0) {
    return VIDEO_PALETTE_YUV422P;

  }else if(strcmp(palette,"YUV411P")==0) {
    return VIDEO_PALETTE_YUV411P;

  }else if(strcmp(palette,"YUV420P")==0) {
    return VIDEO_PALETTE_YUV420P;

  }else if(strcmp(palette,"YUV410P")==0) {
    return VIDEO_PALETTE_YUV410P;
  }
  
  POST_WARNING("Unknown palette type\n");
  return 0;
}

////////////////////////////////////////////////////////////////////////////
/*! Set a new palette in the capture device
 *  @param *viddev : a pointer to the video capture device structure
 *  @param *palette : a string for palette
 *  @return FLGR_Ret (FLGR_RET_OK, ..., FLGR_RET_UNDEFINED_ERROR, ...)
 */
////////////////////////////////////////////////////////////////////////////
FLGR_Ret flgr2d_rtcapture_set_palette(FLGR_RtCapture2D *viddev, char *palette) {
  unsigned short plte = flgr2d_rtcapture_palette_from_string(palette);

  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return FLGR_RET_NULL_OBJECT;
  }

  if(plte==0) return FLGR_RET_UNDEFINED_ERROR;

  viddev->vpic.palette = plte;

  if( ioctl (viddev->fd, VIDIOCSPICT, &(viddev->vpic)) < 0 ) {
    POST_ERROR("");
    perror("VIDIOCSPICT");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  return FLGR_RET_OK;
}



#define ALLOC_CPY_STRING(var,string)		\
  var = (char *) malloc(strlen(string)+1);	\
  strcpy(var,string)

////////////////////////////////////////////////////////////////////////////
/*! Get the palette of a capture device
 *  @param *viddev : a pointer to the video capture device structure
 *  @return string for palette
 */
////////////////////////////////////////////////////////////////////////////
char *flgr2d_rtcapture_get_palette(FLGR_RtCapture2D *viddev) {
  char *str;

  FLGR_DECLARE_FUNCTION;
  
  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return NULL;
  }

  switch(viddev->vpic.palette) {
  case VIDEO_PALETTE_GREY:
    ALLOC_CPY_STRING(str,"GREY");break;

  case VIDEO_PALETTE_HI240:
    ALLOC_CPY_STRING(str,"HI240");break;

  case VIDEO_PALETTE_RGB565:
    ALLOC_CPY_STRING(str,"RGB565");break;

  case VIDEO_PALETTE_RGB24:
    ALLOC_CPY_STRING(str,"RGB24");break;

  case VIDEO_PALETTE_RGB32:
    ALLOC_CPY_STRING(str,"RGB32");break;

  case VIDEO_PALETTE_RGB555:
    ALLOC_CPY_STRING(str,"RGB555");break;

  case VIDEO_PALETTE_YUV422:
    ALLOC_CPY_STRING(str,"YUV422");break;

  case VIDEO_PALETTE_YUYV:
    ALLOC_CPY_STRING(str,"YUYV");break;

  case VIDEO_PALETTE_UYVY:
    ALLOC_CPY_STRING(str,"UYVY");break;

  case VIDEO_PALETTE_YUV420:
    ALLOC_CPY_STRING(str,"YUV420");break;

  case VIDEO_PALETTE_YUV411:
    ALLOC_CPY_STRING(str,"YUV411");break;

  case VIDEO_PALETTE_RAW:
    ALLOC_CPY_STRING(str,"RAW");break;

  case VIDEO_PALETTE_YUV422P:
    ALLOC_CPY_STRING(str,"YUV422P");break;

  case VIDEO_PALETTE_YUV411P:
    ALLOC_CPY_STRING(str,"YUV411P");break;

  case VIDEO_PALETTE_YUV420P:
    ALLOC_CPY_STRING(str,"YUV420P");break;

  case VIDEO_PALETTE_YUV410P:
    ALLOC_CPY_STRING(str,"YUV410P");break;

  default:
    ALLOC_CPY_STRING(str,"UNKNOWN FORMAT");break;    
  }
  
  return str;
}





////////////////////////////////////////////////////////////////////////////
/*! Create the structure FLGR_RtCapture2D and initialize video capture device
 *  @param *device : string for path to the video capture device
 *  @return a pointer to the FLGR_RtCapture2D structure
 */
////////////////////////////////////////////////////////////////////////////
FLGR_RtCapture2D *flgr2d_rtcapture_create(char *device) {
  int i;
  char *str;
  FLGR_RtCapture2D *viddev = malloc(sizeof(FLGR_RtCapture2D));

  FLGR_DECLARE_FUNCTION;

  viddev->fd = open(device, O_RDWR);
  if(viddev->fd<0) {
    POST_ERROR("Could not open %s! ",device);
    perror("open");
    free(viddev);
    return NULL;
  }

  if( ioctl(viddev->fd, VIDIOCGCAP, &(viddev->vcap) ) < 0 ) {
    POST_ERROR("%s is not a v4l device ??? ",device);
    perror("VIDIOGCAP");
    close(viddev->fd);
    free(viddev);
    return NULL;
  }

  if( ioctl(viddev->fd, VIDIOCGWIN, &(viddev->vwin)) < 0 ) {
    POST_ERROR("");
    perror("VIDIOCGWIN");
    close(viddev->fd);
    free(viddev);
    return NULL;
  }

  POST_INFO("Capture Device found : %s\n",viddev->vcap.name);
  POST_INFO("Capture Device Size : max size_x:%d  max size_y:%d\n",viddev->vcap.maxwidth, viddev->vcap.maxheight);

  if( ioctl(viddev->fd, VIDIOCGPICT, &(viddev->vpic)) < 0 ) {
    POST_ERROR("");
    perror("VIDIOCGPICT");
    close(viddev->fd);
    free(viddev);
    return NULL;
  }


  if( ioctl(viddev->fd, VIDIOCGCHAN, &(viddev->vchan)) < 0 ) {
    POST_WARNING("Capture device does not support Get Video Channel IOCTL\n");
  } else {    
    POST_INFO("Video Channel found: %s \n", viddev->vchan.name);
  }


  POST_INFO("Current palette : %s\n",flgr2d_rtcapture_get_palette(viddev));

/*   POST_INFO("Change Capture resolution to maximum capabilities"); */
/*   viddev->vwin.width = viddev->vcap.maxwidth; */
/*   viddev->vwin.height = viddev->vcap.maxheight; */
/*   if( ioctl (viddev->fd, VIDIOCSWIN, &(viddev->vwin)) < 0 ) { */
/*     POST_WARNING("Could not change capture resolution to %d x %d \n", */
/* 		 viddev->vcap.maxwidth, viddev->vcap.maxheight);	       */
/*   } */

  POST_INFO("MMAP Frame Buffer\n");
  memset (&(viddev->vmbuf), 0, sizeof (viddev->vmbuf));
  
  if( ioctl(viddev->fd, VIDIOCGMBUF, &(viddev->vmbuf)) < 0 ) {
    POST_ERROR("");
    perror (" init VIDIOCGMBUF FAILED\n");
  }

  POST_INFO("Video MBUF Size %d ; Frames %d ; Offsets[0]=%d ; Offsets[1]=%d\n",
	    viddev->vmbuf.size, viddev->vmbuf.frames,
	    viddev->vmbuf.offsets[0], viddev->vmbuf.offsets[1]);

  viddev->framebuffer = (unsigned char *) mmap (0, viddev->vmbuf.size, PROT_READ | PROT_WRITE, MAP_SHARED, viddev->fd, 0);

  viddev->vmmap.width = viddev->vwin.width;
  viddev->vmmap.height = viddev->vwin.height;
  viddev->vmmap.format = viddev->vpic.palette;

  POST_INFO("Capture bps: %d ; palette: %d\n",viddev->vpic.depth, viddev->vpic.palette);
  POST_INFO("Pixel Format : %s\n",str=flgr2d_rtcapture_get_palette(viddev));
  free(str);

  for(i=0 ; i<viddev->vmbuf.frames ; i++) {
    viddev->vmmap.frame = i;
    if( ioctl (viddev->fd, VIDIOCMCAPTURE, &(viddev->vmmap)) ) {
      POST_WARNING("");
      perror("CMCAPTURE");
    }    
  }
  viddev->vmmap.frame = 0;

  return viddev;
}

////////////////////////////////////////////////////////////////////////////
/*! Destroy the structure FLGR_RtCapture2D and close correctly the video capture device
 *  @param *viddev : a pointer to the video capture device structure
 *  @return void
 */
////////////////////////////////////////////////////////////////////////////
void flgr2d_rtcapture_destroy(FLGR_RtCapture2D *viddev) {
  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return;
  }

  munmap (viddev->framebuffer, viddev->vmbuf.size);
  close(viddev->fd);
  free(viddev);
}

////////////////////////////////////////////////////////////////////////////
/*! Change the window size of the video capture device
 *  @param *viddev : a pointer to the video capture device structure
 *  @param size_x
 *  @param size_y
 *  @return FLGR_Ret (FLGR_RET_OK, ..., FLGR_RET_UNDEFINED_ERROR, ...)
 */
////////////////////////////////////////////////////////////////////////////
FLGR_Ret flgr2d_rtcapture_set_window_size(FLGR_RtCapture2D *viddev, int size_x, int size_y) {
  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return FLGR_RET_NULL_OBJECT;
  }

  if(size_x>viddev->vcap.maxwidth) {
    POST_ERROR("size_x too large\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  if(size_y>viddev->vcap.maxheight) {
    POST_ERROR("size_x too large\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  viddev->vwin.width = size_x;
  viddev->vwin.height = size_y;
  viddev->vmmap.width = size_x;
  viddev->vmmap.height = size_y;

  if( ioctl (viddev->fd, VIDIOCSWIN, &(viddev->vwin)) < 0 ) {
    POST_ERROR("Could not change capture resolution to %d x %d \n",
		 size_x, size_y);
    return FLGR_RET_UNDEFINED_ERROR;
  }

  if( viddev->vwin.width != size_x) {
    POST_ERROR("Width capture resolution change failed!\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  if( viddev->vwin.height != size_y) {
    POST_ERROR("Height capture resolution change failed!\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  
  
  return FLGR_RET_OK;
}

////////////////////////////////////////////////////////////////////////////
/*! get the window size of the video capture device
 *  @param *viddev : a pointer to the video capture device structure
 *  @param *size_x : pointer to size_x variable
 *  @param *size_y : pointer to size_y variable
 *  @return FLGR_Ret (FLGR_RET_OK, ..., FLGR_RET_UNDEFINED_ERROR, ...)
 */
////////////////////////////////////////////////////////////////////////////
FLGR_Ret flgr2d_rtcapture_get_window_size(FLGR_RtCapture2D *viddev, int *size_x, int *size_y) {
  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return FLGR_RET_NULL_OBJECT;
  }

  if( ioctl (viddev->fd, VIDIOCGWIN, &(viddev->vwin)) < 0 ) {
    POST_ERROR("Could not get capture resolution\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }

  *size_x = viddev->vwin.width;
  *size_y = viddev->vwin.height; 
  
  return FLGR_RET_OK;
}

////////////////////////////////////////////////////////////////////////////
/*! Capture a gray image from a video capture device
 *  @param *img : a pointer to the image structure (image must be fgUINT8)
 *  @param *viddev : a pointer to the video capture device structure
 *  @return FLGR_Ret (FLGR_RET_OK, ..., FLGR_RET_UNDEFINED_ERROR, ...)
 */
////////////////////////////////////////////////////////////////////////////
FLGR_Ret flgr2d_rtcapture_gray_image(FLGR_Data2D *img, FLGR_RtCapture2D *viddev) {
  int framec;
  unsigned char *ptr;
  fgUINT8 *rowptr;
  int tmp;
  int i,j,k;

  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return FLGR_RET_NULL_OBJECT;
  }

  if((viddev->vwin.width != img->size_x) || (viddev->vwin.height != img->size_y)){
    POST_ERROR("Capture Window sizes and Images sizes different\n");
    return FLGR_RET_SIZE_ERROR;
  }

  if(img->type != FLGR_UINT8) {
    POST_ERROR("type is not allowed for gray capture\n");
    return FLGR_RET_TYPE_UNKNOWN;
  }

  if( (viddev->vpic.palette != VIDEO_PALETTE_YUV420P ) && 
      (viddev->vpic.palette != VIDEO_PALETTE_RGB32   ) && 
      (viddev->vpic.palette != VIDEO_PALETTE_RGB24   ) ){
    POST_ERROR("Capture device could not work with Gray images\n");
    return FLGR_RET_UNDEFINED_ERROR;
  }


  framec = viddev->vmmap.frame;
  if( ioctl (viddev->fd, VIDIOCSYNC, &framec) < 0 ) {
    POST_ERROR("");
    perror ("Capture sync error");
    return FLGR_RET_UNDEFINED_ERROR;
  }
  
  viddev->vmmap.frame = framec;  
  ptr = viddev->framebuffer + viddev->vmbuf.offsets[viddev->vmmap.frame];


  if( viddev->vpic.palette == VIDEO_PALETTE_YUV420P ) {
    for(i=0 ; i<img->size_y ; i++) {
      memcpy(img->array[i], ptr, img->size_x);
      ptr += img->size_x;
    }
  }else if( viddev->vpic.palette == VIDEO_PALETTE_RGB32 ) {
    for(i=0 ; i<img->size_y ; i++) {
      rowptr = img->array[i];
      k=0;
      for(j=0 ; j<img->size_x ; j++) {
	tmp = ptr[k++];
	tmp += ptr[k++];
	tmp += ptr[k++];
	rowptr[j]=tmp/3;
      }
      ptr += img->size_x*4;
    }
  }else {
    for(i=0 ; i<img->size_y ; i++) {
      rowptr = img->array[i];
      k=0;
      for(j=0 ; j<img->size_x ; j++) {
	tmp = ptr[k++];
	tmp += ptr[k++];
	tmp += ptr[k++];
	rowptr[j]=tmp/3;
      }
      ptr += img->size_x*3;
    }
  }
  
  if((ioctl (viddev->fd, VIDIOCMCAPTURE, &(viddev->vmmap))) < 0 ) {
    POST_ERROR("");
    perror ("cmcapture");
    return FLGR_RET_UNDEFINED_ERROR;
  }
  viddev->vmmap.frame = (viddev->vmmap.frame + 1) % viddev->vmbuf.frames;
  
  return FLGR_RET_OK;
  
}

////////////////////////////////////////////////////////////////////////////
/*! Capture a RGB image from a video capture device
 *  @param *imgR : a pointer to the image structure for Red (image must be fgUINT8)
 *  @param *imgG : a pointer to the image structure for Green (image must be fgUINT8)
 *  @param *imgB : a pointer to the image structure for Blue (image must be fgUINT8)
 *  @param *viddev : a pointer to the video capture device structure
 *  @return FLGR_Ret (FLGR_RET_OK, ..., FLGR_RET_UNDEFINED_ERROR, ...)
 */
////////////////////////////////////////////////////////////////////////////
FLGR_Ret flgr2d_rtcapture_RGB24_image(FLGR_Data2D *imgR, FLGR_Data2D *imgG, FLGR_Data2D *imgB, FLGR_RtCapture2D *viddev) {
  int i,j,k;
  int framec;
  unsigned char *ptr;
  FLGR_Ret ret;
  fgUINT8 *ptrR,*ptrG,*ptrB;
  
  FLGR_DECLARE_FUNCTION;

  if(viddev==NULL) {
    POST_ERROR("Null objects!\n");
    return FLGR_RET_NULL_OBJECT;
  }

  if(imgR->type != FLGR_UINT8) {
    POST_ERROR("ype is not allowed for gray capture\n");
    return FLGR_RET_TYPE_UNKNOWN;
  }
  if(imgG->type != FLGR_UINT8) {
    POST_ERROR("type is not allowed for gray capture\n");
    return FLGR_RET_TYPE_UNKNOWN;
  }
  if(imgB->type != FLGR_UINT8) {
    POST_ERROR("type is not allowed for gray capture\n");
    return FLGR_RET_TYPE_UNKNOWN;
  }
  
  if( (ret = flgr2d_is_data_same_size(imgR,imgG)) != FLGR_RET_OK ) {
    POST_ERROR("Images have different sizes!\n");
    return FLGR_RET_SIZE_ERROR;
  }

  if( (ret = flgr2d_is_data_same_size(imgR,imgB)) != FLGR_RET_OK ) {
    POST_ERROR("Images have different sizes!\n");
    return FLGR_RET_SIZE_ERROR;
  }



  if( viddev->vpic.palette == VIDEO_PALETTE_RGB24 ) {
    framec = viddev->vmmap.frame;
    if( ioctl (viddev->fd, VIDIOCSYNC, &framec) < 0 ) {
      POST_ERROR("");
      perror ("Capture sync error");
      return FLGR_RET_UNDEFINED_ERROR;
    }

    viddev->vmmap.frame = framec;

    ptr = viddev->framebuffer + viddev->vmbuf.offsets[viddev->vmmap.frame];
    for(i=0 ; i<imgR->size_y ; i++) {
      ptrR = imgR->array[i];
      ptrG = imgG->array[i];
      ptrB = imgB->array[i];
      k=0;
      for(j=0 ; j<imgR->size_x ; j++) {
	ptrR[j] = (fgUINT8) ptr[k++];
	ptrG[j] = (fgUINT8) ptr[k++];
	ptrB[j] = (fgUINT8) ptr[k++];
      }
      ptr += (imgR->size_x*3);
    }

    if((ioctl (viddev->fd, VIDIOCMCAPTURE, &(viddev->vmmap))) < 0 ) {
      POST_ERROR("");
      perror ("cmcapture");
      return FLGR_RET_UNDEFINED_ERROR;
    }
    viddev->vmmap.frame = (viddev->vmmap.frame + 1) % viddev->vmbuf.frames;
    
    return FLGR_RET_OK;
    
  }else {
    POST_ERROR("Capture device could not work with Gray images\n");
    return FLGR_RET_UNDEFINED_ERROR;

  }
}


//! @}
